#include "Spring.h"

namespace poly
{
	Spring::Spring() :
		rigidBody1( 0 ), rigidBody2( 0 ), forceDensity( 0 ), restLength( 0 ), friction( 0 ),
			pointOfApp1( 0, 0, 0 ), pointOfApp2( 0, 0, 0 )
	{
	}

	void Spring::apply(float dt)
	{
		float3 pt1 = transformCoord( pointOfApp1, rigidBody1->world() );
		float3 pt2 = transformCoord( pointOfApp2, rigidBody2->world() );

		float3 d = pt2 - pt1;
		float3 u = normalize( d );

		float3 vec_v1 = rigidBody1->vel + cross(rigidBody1->rotVel, pt1-rigidBody1->pos);
		float3 vec_v2 = rigidBody2->vel + cross(rigidBody2->rotVel, pt2-rigidBody2->pos);

		float v1 = dot(vec_v1, u);
		float v2 = dot(vec_v2, u);

		float l = length( d );
		float x = l - restLength;

		float3 force = x * forceDensity * u + u * (v2-v1) * friction;
		rigidBody1->addForce( +force, pt1 );
		rigidBody2->addForce( -force, pt2 );
	}
}
